#include "ros/ros.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
/*
    订阅方实现
*/
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;

    //创建订阅者对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);


    ros::Rate rate(10);
    //编写发布逻辑
    geometry_msgs::PointStamped p1;
    p1.header.stamp = ros::Time::now();
    p1.header.frame_id = "son1";
    p1.point.x = 1.0;
    p1.point.y = 2.0;
    p1.point.z = 5.0;
    while(ros::ok())
    {
        try
        {
            //son1相对son2
            geometry_msgs::TransformStamped son1toson2 = buffer.lookupTransform("son2","son1",ros::Time(0));
            
            //得到son1点在son2点中的坐标位置
            geometry_msgs::PointStamped p2 = buffer.transform(p1,"son2");
            ROS_INFO("在son2的坐标是：%.2f %.2f %.2f  相对坐标是：%s",
                                p2.point.x,
                                p2.point.y,
                                p2.point.z,
                                p2.header.frame_id.c_str()
            );
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示“%s",e.what());
        }
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}
